#include "SM.h"
#include "DL_Service.h"
#include "IO_LINK_DataBuffer.h"
#include "math.h"
#include "Phy_Driver.h"
#include "AL_Service.h"
#include "AL_OD.h"
#include "CM.h"

/********************
     暂时排除在工程外 8.3

****************/

SM_DL_STATE_TYPE SM_DL_STATE[PORT_NUM]; //SM中记录的DL状态，由DL_Mode.ind申报
COM_Speed_TYPE SM_COM_Speed[PORT_NUM];//SM中记录DL的速度，由DL_Mode.ind申报

//SM状态机控制变量
SM_STATE_TYPE SM_STATE[PORT_NUM];
uint8_t SM_CompRetry[PORT_NUM];
BOOL  sRead_Flag[PORT_NUM];
//保存子状态的零时变量，此变量仅用于保存进入CheckComp_23和CheckCompV10_21后，进入读取
//Page参数的子状态 
 
//Event_Page_cnf是为了读取Page参数的返回值，为了连续读取多个参数，采用了自定义sub状态链，
//每个子状态负责读取一个Page参数
SM_STATE_TYPE SM_sub_STATE[PORT_NUM];


//从Device读出的参数
uint16_t MinCycleTime[PORT_NUM];
uint8_t M_SquenceCapability[PORT_NUM];
uint8_t RevisionID[PORT_NUM];
uint8_t ProcessDataIn[PORT_NUM];
uint8_t ProcessDataOut[PORT_NUM];
uint16_t VendorID[PORT_NUM];
uint32_t DeviceID[PORT_NUM];
uint16_t FunctionID[PORT_NUM];

uint8_t Serial_Number[PORT_NUM][16];

// SM Service配置的参数
SM_InspectionLevel_TYPE IL[PORT_NUM];
uint8_t CRID[PORT_NUM];
uint16_t CVID[PORT_NUM];
uint32_t CDID[PORT_NUM];
uint16_t CFID[PORT_NUM];
uint8_t CSN[PORT_NUM][16];
uint16_t  CfgCycleTime[PORT_NUM];
SM_TargetMode_TYPE TargetMode[PORT_NUM];

/**********************Callback*******************/
 void  Read_SerNum_CallBack(uint8_t Port, uint8_t * Data, uint8_t DataLen, uint16_t ErrorInfo){
     if(ErrorInfo==0x0000){
          memcpy(Serial_Number,Data,DataLen); //拷贝读出的序列号
          Event_SM_SRead_OK(Port)
     }else{
          Event_SM_SRead_Nagtive(Port);
     }
 }
 

//**********************service**********************/

uint16_t SM_GetPortConfig(SM_GetPortConfig_Arg_TYPE Arg){

}


/*******************Event***************************/



void Event_SM_Init(void){
    for(uint8_t i=0;i<PORT_NUM;i++){
       SM_STATE[i]=SM_PortInactive_0;
      
    }
}
 
void Event_DL_Mode_STARTUP(uint8_t Port){
    if(SM_STATE[Port]==SM_PortInactive_0){
        SM_STATE[Port]=SM_ReadComParameter_20;
        SM_T1(Port);
    }

}

void Event_SM_WriteDone_BOOL(uint8_t Port){
   SM_STATE[Port]==SM_ReadComParameter_20;
   SM_T24(Port);
}

void Event_SM_SRead_Nagtive(uint8_t Port){
    if(SM_STATE[Port]==SM_ReadSerNum_30){
          
        SM_STATE[Port]=SM_CheckSerNum_31;
        sRead_Flag[Port]=False;
        SM_T30_31(Port);

    }
}

void Event_SM_SRead_OK(uint8_t Port){
    if(SM_STATE[Port]==SM_ReadSerNum_30){
        
       SM_STATE[Port]=SM_CheckSerNum_31;
       sRead_Flag[Port]=True;
       SM_T30_31(Port);
    }
}

//此事件驱动状态从自定义子状态中返回
void Event_Page_Cnf(uint8_t Port){
      // RestartDevice中的状态动作(CFGCOM) ,参见图76
    if(SM_sub_STATE[Port]==SM_sub_Write_Page_0x04&&TargetMode[Port]==SM_CFGCOM&&SM_STATE[Port]==SM_RestartDevice_24){
        SM_sub_STATE[Port]=SM_sub_Write_Page_0x09;
        uint8_t CDID_MSB2=(CDID[Port]&0x00FF0000)>>16;
        Service_DL_Write(Port,0x09,CDID_MSB2);
   }
   else if(SM_sub_STATE[Port]==SM_sub_Write_Page_0x09&&TargetMode[Port]==SM_CFGCOM&&SM_STATE[Port]==SM_RestartDevice_24){
       SM_sub_STATE[Port]=SM_sub_Write_Page_0x0A;
       uint8_t CDID_MSB1=(CDID[Port]&0x0000FF00)>>8;
       Service_DL_Write(Port,0x0A,CDID_MSB1);
   }
   else if(SM_sub_STATE[Port]==SM_sub_Write_Page_0x0A&&TargetMode[Port]==SM_CFGCOM&&SM_STATE[Port]==SM_RestartDevice_24){
       SM_sub_STATE[Port]=SM_sub_Write_Page_0x0B;
       uint8_t CDID_LSB=CDID[Port]&0x000000FF;
       Service_DL_Write(Port,0x0B,CDID_LSB);       
   }
   else if(SM_sub_STATE[Port]==SM_sub_Write_Page_0x0B&&TargetMode[Port]==SM_CFGCOM&&SM_STATE[Port]==SM_RestartDevice_24){
       SM_sub_STATE[Port]=SM_sub_Read_Page_0x02;
       Service_DL_Read(Port,0x02);//dummy cycle??
   }
   else if(SM_sub_STATE[Port]==SM_sub_Read_Page_0x02&&TargetMode[Port]==SM_CFGCOM&&SM_STATE[Port]==SM_RestartDevice_24){
       MinCycleTime[Port]=PAGE_BufferIn[Port]; //dummy cylcle?
        SM_sub_STATE[Port]=0;//清空Sub子状态
        Event_SM_WriteDone_BOOL(Port); //T24跳转
   }
    // RestartDevice中的状态动作(AUTOCOM) ,参见图76
   else if(SM_sub_STATE[Port]==SM_sub_Write_Page_0x04&&TargetMode[Port]==SM_AUTOCOM){
         SM_sub_STATE[Port]=SM_sub_Write_Page_0x00;
         Service_DL_Write(Port,0x00,0x96);
          
   }else if(SM_sub_STATE[Port]==SM_sub_Write_Page_0x00&&TargetMode[Port]==SM_AUTOCOM){
        SM_sub_STATE[Port]=SM_sub_Read_Page_0x02;
        Service_DL_Read(Port,0x02);
   }else if(SM_sub_STATE[Port]==SM_sub_Read_Page_0x02&&TargetMode[Port]==SM_AUTOCOM&&SM_STATE[Port]==SM_RestartDevice_24){
        MinCycleTime[Port]=PAGE_BufferIn[Port]; //dummy cylcle?
        SM_sub_STATE[Port]=0;//清空Sub子状态
        Event_SM_WriteDone_BOOL(Port); //T24跳转

   }


   else if(SM_sub_STATE[Port]==SM_sub_Read_Page_0x02){
         MinCycleTime[Port]=PAGE_BufferIn[Port]; //page    
       SM_sub_STATE[Port]=SM_sub_Read_Page_0x03;    
        Service_DL_Read(Port,0x03); 
   }else if(SM_sub_STATE[Port]==SM_sub_Read_Page_0x03){
        M_SquenceCapability[Port]=PAGE_BufferIn[Port];  
        SM_sub_STATE[Port]=SM_sub_Read_Page_0x04;    
        Service_DL_Read(Port,0x04); 
   }else if(SM_sub_STATE[Port]==SM_sub_Read_Page_0x04){
         RevisionID[Port]=PAGE_BufferIn[Port];  
        SM_sub_STATE[Port]=SM_sub_Read_Page_0x05;    
        Service_DL_Read(Port,0x05); 
   }else if(SM_sub_STATE[Port]==SM_sub_Read_Page_0x05){
        ProcessDataIn[Port]=PAGE_BufferIn[Port];  
        SM_sub_STATE[Port]=SM_sub_Read_Page_0x06;    
        Service_DL_Read(Port,0x06); 
   }else if(SM_sub_STATE[Port]==SM_sub_Read_Page_0x06){
        ProcessDataOut[Port]=PAGE_BufferIn[Port];  
        SM_sub_STATE[Port]=SM_ReadComParameter_20;//读完Page参数，回到ReadComParameter状态。
        //执行参数检查任务
        if(RevisionID[Port]==0x01){
            Event_equel_V10(Port);
        }else{
            Event_not_equal_V10(Port);
        }
   }else if(SM_sub_STATE[Port]==SM_sub_Read_Page_0x07){
        uint16_t MSB=(uint16_t)PAGE_BufferIn[Port];
        VendorID[Port]=MSB<<8;
        SM_sub_STATE[Port]=SM_sub_Read_Page_0x08;
        Service_DL_Read(Port,0x08);
   }else if(SM_sub_STATE[Port]==SM_sub_Read_Page_0x08){
       uint16_t LSB=(uint16_t)PAGE_BufferIn[Port];
       VendorID[Port]|=LSB;
       SM_sub_STATE[Port]=SM_sub_Read_Page_0x09;
       Service_DL_Read(Port,0x09);
   }else if(SM_sub_STATE[Port]==SM_sub_Read_Page_0x09){
       uint32_t MSB=(uint32_t)PAGE_BufferIn[Port];
       DeviceID[Port]=MSB<<16;
       SM_sub_STATE[Port]=SM_sub_Read_Page_0x0A;
       Service_DL_Read(Port,0x0A);
   }else if(SM_sub_STATE[Port]==SM_sub_Read_Page_0x0A){
       uint32_t MSB=(uint32_t)PAGE_BufferIn[Port];
       DeviceID[Port]|=MSB<<8;
       SM_sub_STATE[Port]=SM_sub_Read_Page_0x0B;
       Service_DL_Read(Port,0x0B);
   }else if(SM_sub_STATE[Port]==SM_sub_Read_Page_0x0B){
       uint32_t LSB=(uint32_t)PAGE_BufferIn[Port];
       DeviceID[Port]|=LSB;
       SM_sub_STATE[Port]=SM_sub_Read_Page_0x0C;
       Service_DL_Read(Port,0x0C);
   }else if(SM_sub_STATE[Port]==SM_sub_Read_Page_0x0C){
       uint16_t MSB=(uint16_t)PAGE_BufferIn[Port];
       FunctionID[Port]=MSB<<8;
       SM_sub_STATE[Port]=SM_sub_Read_Page_0x0D;
       Service_DL_Read(Port,0x0D);
   }else if(SM_sub_STATE[Port]==SM_sub_Read_Page_0x0D)
   {
       uint16_t LSB=(uint16_t)PAGE_BufferIn[Port];
       FunctionID[Port]|=LSB;
       if(SM_STATE[Port]==SM_CheckCompV10_21){
           SM_sub_STATE[Port]=SM_CheckCompV10_21;//读取参数完毕 ，返回checkCompV10
           Action_do_check_comp_V10(Port);
           return;
       }
       if(SM_STATE[Port]==SM_CheckComp_23){
           SM_sub_STATE[Port]=SM_CheckComp_23;
           Action_do_check_comp(Port);
           return;
       }
       
    
   }
   else{
       return;
   }
   
}

void Event_equal_V10(uint8_t Port){
   if(SM_STATE[Port]==SM_ReadComParameter_20){
       SM_STATE[Port]=SM_CheckCompV10_21; 
       SM_T20(Port);
   }
}

void Event_not_equal_V10(uint8_t Port){
   if(SM_STATE[Port]==SM_ReadComParameter_20){
       SM_STATE[Port]=SM_CheckVxy_22;
       SM_T21(Port);
   }
}

 

void Event_DL_Mode_PREOPERATE(uint8_t Port){
    if(SM_STATE[Port]==SM_waitonDLPreoperate_2||SM_STATE[Port]==SM_waitonDLOperate_7){
        //SM_STATE[Port]=SM_checkSerNum_3;
        SM_STATE[Port]=SM_ReadSerNum_30;
        SM_T8_9(Port); //调用SM_ReadSerNum_30的状态动作
    }
}

void Event_DL_Mode_OPERATE(uint8_t Port){
    if(SM_STATE[Port]==SM_SMOperate_5){
       
       // SM_T13(Port);
        
    }
}

void Event_DL_Mode_COMLOST(uint8_t Port){
    SM_STATE[Port]=SM_PortInactive_0;
    SM_T3(Port);
}

void Event_SM_WriteDone_BOOL(uint8_t Port){
    if(SM_STATE[Port]==SM_RestartDevice_24){
        SM_STATE[Port]=SM_ReadComParameter_20;

        SM_sub_STATE[Port]=SM_sub_Read_Page_0x02;  //进入读参数子状态链  
        Service_DL_Read(Port,0x02);
    }
}

void Event_SerNumFault_BOOL(uint8_t Port){
    if(SM_STATE[Port]==SM_CheckSerNum_31){
        SM_STATE[Port]=SM_InspectionFault_6;
        SM_T11(Port);
    }
}

void Event_SerNumOK_BOOL(uint8_t Port){
    if(SM_STATE[Port]==SM_CheckSerNum_31){
        SM_STATE[Port]=SM_wait_4;
        SM_T10(Port);
    }
}


/**********************状态动作*********************/
 
void SM_T1(uint8_t Port){
    SM_CompRetry[Port]=0;
    //ReadComParameter状态动作,首先读取Page地址0x02
    //进入自定义子状态
    SM_sub_STATE[Port]=SM_sub_Read_Page_0x02;    
    Service_DL_Read(Port,0x02);

}

void SM_T2(uint8_t Port){
    DL_SetMode_ValueList_StructTypeDef args;
    Create_DL_SetMode_Auguments_For_PreOperate(&args);
    Service_DL_SetMode(Port,PREOPERATE,args);

    
    
}

void SM_T3(uint8_t Port){
    DL_SetMode_ValueList_StructTypeDef args;
    Create_DL_SetMode_Auguments_For_INACTIVE(&args);
    Service_DL_SetMode(Port,INACATIVE,args);
    
}

//当版本为V1.0时候，才调用此函数
void SM_T4(uint8_t Port){
    DL_SetMode_ValueList_StructTypeDef args;
    Create_DL_SetMode_Auguments_For_OPERATE_V1_0(&args);
    Service_DL_SetMode(Port,OPERATE,args);
}

void SM_T5(uint8_t Port){
    Service_SM_PortMode(Port,SM_Mode_COMP_FAULT);
    DL_SetMode_ValueList_StructTypeDef args;
    Create_DL_SetMode_Auguments_For_PreOperate(&args);
    //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@//
    //@此处更改协议，发送DL_SetMode_PreOperate,
    //源协议为发送DL_SetMode_Operate
    Service_DL_SetMode(Port,PREOPERATE,args); 
}

void SM_T6(uint8_t Port){
    Service_SM_PortMode(Port,SM_Mode_REVISION_FAULT);
    DL_SetMode_ValueList_StructTypeDef args;
    Create_DL_SetMode_Auguments_For_PreOperate(&args);
    Service_DL_SetMode(Port,PREOPERATE,args);
}

void SM_T7(uint8_t Port){
     Service_SM_PortMode(Port,SM_Mode_COMP_FAULT);

     DL_SetMode_ValueList_StructTypeDef args;
    Create_DL_SetMode_Auguments_For_PreOperate(&args);
    Service_DL_SetMode(Port,PREOPERATE,args);

}

void SM_T8_9(uint8_t Port){
     Service_AL_Read_Req(Port,0x0015,0x00);
}

void SM_T10(uint8_t Port){
   Service_SM_PortMode(Port,SM_Mode_COMREADY);
}

void SM_T11(uint8_t Port){
   Service_SM_PortMode(Port,SM_Mode_SERNUM_FAULT);
}
 
 

void SM_T12(uint8_t Port){
    DL_SetMode_ValueList_StructTypeDef args;
        if(RevisionID[Port]==0x10){
            Create_DL_SetMode_Auguments_For_OPERATE_V1_0(&args);
        }else{
            Create_DL_SetMode_Auguments_For_OPERATE_V1_1(&args);
        }
        
        Service_DL_SetMode(Port,OPERATE,args); 
}


 

void SM_T14(uint8_t Port){
    Service_SM_PortMode(Port,SM_INACTIVE);
    DL_SetMode_ValueList_StructTypeDef args;
    Create_DL_SetMode_Auguments_For_INACTIVE(&args);
    Service_DL_SetMode(Port,INACATIVE,args);
}

void SM_T15(uint8_t Port){
   //将IO接口切换到IO-LINK模式
   Service_PL_SetMode_SDCI(Port);

    DL_SetMode_ValueList_StructTypeDef args;
    Create_DL_SetMode_Auguments_For_STARTUP(&args);
    Service_DL_SetMode(Port,STARTUP,args);
     
}

void SM_T16_DI(uint8_t Port){
 //将IO接口切换到普通IO模式
   Service_PL_SetMode_DI(Port);
   
   Service_SM_PortMode(Port,SM_Mode_DI);
   
}

void SM_T16_DO(uint8_t Port){
 //将IO接口切换到普通IO模式
   Service_PL_SetMode_DO(Port);
   
   Service_SM_PortMode(Port,SM_Mode_DO);
   
}


void SM_T17(uint8_t Port){
    Service_SM_PortMode(Port,SM_CYCTIME_FAULT);

    DL_SetMode_ValueList_StructTypeDef args;
    Create_DL_SetMode_Auguments_For_PreOperate(&args);
    Service_DL_SetMode(Port,PREOPERATE,args);
}

void SM_T18(uint8_t Port){
    Service_SM_PortMode(Port,SM_CYCTIME_FAULT);
     DL_SetMode_ValueList_StructTypeDef args;
    Create_DL_SetMode_Auguments_For_PreOperate(&args);
    //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    //此处更改协议 
    //源协议为 DL_SetMode_INACTIVE
    Service_DL_SetMode(Port,PREOPERATE,args);
}

void SM_T20(uint8_t Port){
    
    SM_sub_STATE[PORT_NUM]=SM_sub_Read_Page_0x07;//进入读取PAGE参数子状态

    Service_DL_Read(SM_sub_Read_Page_0x07);
}

void SM_T21(uint8_t Port){
    
    Service_DL_Write(Port,0x00,0x95);// 发送命令MasterIdent
    SM_STATE[Port]=SM_CheckVxy_22;
    Action_do_check_revision(Port);
    
}


void SM_T22(uint8_t Port){
    //进入自定义子状态序列，并开始读取Page参数0x07
    SM_STATE[Port]=SM_sub_Read_Page_0x07;
    Service_DL_Read(SM_sub_Read_Page_0x07); 
}

void SM_T23(uint8_t Port){
    SM_STATE[Port]=SM_RestartDevice_24;
    Action_do_write_parameter(Port); //状态RestartDevice_24的动作
}

void SM_T25(uint8_t Port){
     SM_STATE[Port]=SM_RestartDevice_24;
    Action_do_write_parameter(Port); //状态RestartDevice_24的动作
}

void SM_T30_31(uint8_t Port){
  Action_do_check_Serial_Number(Port);
}

 



/***********************action***********************/
void Action_do_check_Serial_Number(uint8_t Port){
    BOOL CSN_Equal_RSN=True;
   //比较CSN和RSN***********************************
    for(uint8_t i=0;i<16;i++){  
        if(CSN[Port][i]!=Serial_Number[Port][i]){
            CSN_Equal_RSN=False;
            break;
        }
    }
  
   //检查序列号
    if(IL[Port]!=SM_IDENTICAL){ //IL不等于IDENTICAL
         Event_SerNumOK_BOOL(Port);
    }
    else if(sRead_Flag[Port]==False){ //未读出字符串
         
        Event_SerNumFault_BOOL(Port);
    }
    else if(CSN_Equal_RSN){ //字符串相等
        
        Event_SerNumOK_BOOL(Port);
    }else{                 //字符串不等
        Event_SerNumFault_BOOL(Port);
    }
 
}

//RestartDevice的状态动作
void Action_do_write_parameter(uint8_t Port){
 
    SM_sub_STATE[Port]=SM_sub_Write_Page_0x04;
    Service_DL_Write(Port,0x04,CRID[Port]);
    
}

void Action_do_check_comp_V10(uint8_t Port){
    if(CfgCycleTime[Port]>0&&CfgCycleTime[Port]<MinCycleTime){
        Event_V10CycTimeFault(Port);
    }
    else if(IL[Port]==SM_NO_CHECK){
        Event_V10CompOK(Port);
    }
    else if(CVID[Port]==VendorID[Port]&&CDID[Port]==DeviceID[Port]){
        Event_V0CompOK(Port);
    }else{
        Event_V10CompFault(Port);
    }

}

void Action_do_check_revision(uint8_t Port){
    if(CRID==RevisionID){
        Event_RevisionOK(Port);
    }else if(SM_CompRetry[Port]==0){
        Event_RetryStartup(Port);
    }else{
        Event_RevisionFault(Port);



    }
}


void Action_do_check_comp(uint8_t Port){
    if(CfgCycleTime[Port]>0&&CfgCycleTime[Port]<MinCycleTime){
        Event_CycTimeFault(Port);
    }
    else if(IL[Port]==SM_NO_CHECK){
       Event_CompOK(Port);
    }
    else if(CVID!=VendorID){
        Event_CompFalut(Port);
    }

    //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    //@此处有更改协议，原协议仅仅在No—CHECK时候OK　不合理
    else if(SM_CompRetry==0&&CDID==DeviceID){

       Event_CompOK(Port); 
    }    
    else if(SM_CompRetry==0&&CDID!=DeviceID){
        Event_RetryStartup(Port);
    }else{
        Event_CompFault(Port);
    }
}






void Event_V10CycTimeFault(uint8_t Port){
    if(SM_STATE[Port]==SM_CheckCompV10_21){
        SM_STATE[Port]=SM_InspectionFault_6;
        SM_T18(Port);
    }
}

void Event_V10CompOK(uint8_t Port){
    if(SM_STATE[Port]==SM_CheckCompV10_21){
        SM_STATE[Port]=SM_waitonDLOperate_7;
        SM_T4(Port);
    }
}

void Event_V10CompFault(uint8_t Port){
    if(SM_STATE[Port]==SM_CheckCompV10_21){
        SM_STATE[Port]=SM_InspectionFault_6;
        SM_T5(Port);
    }
}


void Event_CompOK(uint8_t Port){
    if(SM_STATE[Port]==SM_CheckComp_23){
        SM_STATE[Port]=SM_waitonDLPreoperate_2;
        SM_T2(Port);
    }
}
void Event_CompFault(uint8_t Port){
    if(SM_STATE[Port]==SM_CheckComp_23){
        SM_STATE[Port]=SM_InspectionFault_6;
        SM_T7(Port);
    }
}

void Event_CycTimeFault(uint8_t Port){
    if(SM_STATE[Port]==SM_CheckComp_23){
        SM_STATE[Port]=SM_InspectionFault_6;
        SM_T17(Port);
    }
}


void Event_RetryStartup(uint8_t Port){
    if(SM_STATE[Port]==SM_CheckVxy_22){
        SM_STATE[Port]=SM_RestartDevice_24;
        SM_T25(Port);
        return;
    }
    if(SM_STATE[Port]==SM_CheckComp_23){
        SM_STATE[Port]=SM_RestartDevice_24;
         SM_T23(Port);//调用状态动作
        return;
    }
}

void Event_RevisionOK(uint8_t Port){
    if(SM_STATE[Port]==SM_CheckVxy_22){
        SM_STATE[Port]=SM_CheckComp_23;
        SM_T22(Port);
    }
}

void Event_RevisionFault(uint8_t Port){
    if(SM_STATE[Port]==SM_CheckVxy_22){
        SM_STATE[Port]=SM_InspectionFault_6;
        SM_T6(Port);
    }
}

void Event_SM_SetPortConfig_INACTIVE(uint8_t Port){
    if(SM_STATE[Port]==SM_PortInactive_0||SM_STATE[Port]==SM_wait_4||SM_STATE[Port]==SM_SMOperate_5||SM_STATE[Port]==SM_InspectionFault_6||SM_STATE[Port]==SM_DIDO_8){
        SM_STATE[Port]=SM_PortInactive_0;
        SM_T14(Port);
    }
}

void Event_SM_SetPortConfig_CFGCOM_AUTOCOM(uint8_t Port){
    if(SM_STATE[Port]==SM_PortInactive_0||SM_STATE[Port]==SM_wait_4||SM_STATE[Port]==SM_SMOperate_5||SM_STATE[Port]==SM_InspectionFault_6||SM_STATE[Port]==SM_DIDO_8){
        SM_STATE[Port]=SM_PortInactive_0;
        SM_T15(Port);
    }
}


void Event_SM_SetPortConfig_DI(uint8_t Port){
    if(SM_STATE[Port]==SM_PortInactive_0||SM_STATE[Port]==SM_wait_4||SM_STATE[Port]==SM_SMOperate_5||SM_STATE[Port]==SM_InspectionFault_6||SM_STATE[Port]==SM_DIDO_8){
        SM_STATE[Port]=SM_PortInactive_0;
        SM_T16_DI(Port);
    }
}


void Event_SM_SetPortConfig_DO(uint8_t Port){
    if(SM_STATE[Port]==SM_PortInactive_0||SM_STATE[Port]==SM_wait_4||SM_STATE[Port]==SM_SMOperate_5||SM_STATE[Port]==SM_InspectionFault_6||SM_STATE[Port]==SM_DIDO_8){
        SM_STATE[Port]=SM_PortInactive_0;
        SM_T16_DO(Port);
    }
}
 
void  Create_DL_SetMode_Auguments_For_PreOperate(DL_SetMode_ValueList_StructTypeDef * args){
    
    uint8_t PreOperate_M_Squence_Code= (M_SquenceCapability&0x30)>>4;
    //解析M-SquenceType
    switch (PreOperate_M_Squence_Code)
    {
    case 0:
        args->M_sequnceType=TYPE_0;
        args->ODLength=1;
         
        break;
    case 1:
        args->M_sequnceType=TYPE_1_1_And_TYPE_1_2;
        args->ODLength=2;
        breaks;
    case 2:
        args->M_sequnceType=TYPE_1_V;
        args->ODLength=8;
        break;
    case 3:
         args->M_sequnceType=TYPE_1_V;
         args->ODLength=32;
         break;
    
    default:
        break;
    }
    //解析PDInLen
    args->PDInputLength=0;
    args->PDOutputLength=0; 
    args->M_squenceTime=CfgCycleTime[Port];//CfgCycleTime在通过SM_SetConfig之前已经被CM检验过
    
}


void  Create_DL_SetMode_Auguments_For_INACTIVE(DL_SetMode_ValueList_StructTypeDef * args){
      args->M_sequnceType=TYPE_0;
      args->M_squenceTime=0;
      args->ODLength=1;
      args->PDInputLength=0;
      args->PDOutputLength=0;
}

void  Create_DL_SetMode_Auguments_For_STARTUP(DL_SetMode_ValueList_StructTypeDef * args){
      args->M_sequnceType=TYPE_0;
      args->M_squenceTime=0;
      args->ODLength=1;
      args->PDInputLength=0;
      args->PDOutputLength=0;
}

 //针对V1.0
void Create_DL_SetMode_Auguments_For_OPERATE_V1_0(DL_SetMode_ValueList_StructTypeDef * args){
     
    uint8_t  Operate_M_Squence_Code= (M_SquenceCapability&0x0E)>>1;
    //解析M-SquenceType
    switch (Operate_M_Squence_Code)
    {
    case 0:
        args->M_sequnceType=TYPE_2_1;
        args->ODLength=1;
         
        break;
    case 1:
        args->M_sequnceType=TYPE_1_1_And_TYPE_1_2;
        args->ODLength=2;
        break;   
    
    default:
        break;
    }
    //解析PDInLen
    if(ProcessDataIn&0x80==0x80){ //说明ProcessDataIn长度是以字节为单位
        args->PDInputLength=ProcessDataIn&0x1F;
    }else{
        args->PDInputLength=(uint8_t)ceil((ProcessDataIn&0x1F)/8);
    }
    //解析PDOutLen
    if(ProcessDataOut&0x80==0x80){ //说明ProcessDataIn长度是以字节为单位
        args->PDOutputLength=ProcessDataOut&0x1F;
    }else{
        args->PDOutputLength=(uint8_t)ceil((ProcessDataOut&0x1F)/8);
    }

    if(args->PDInputLength>=3||args->PDOutputLength>=3){
        args->ODLength=2;
        args->M_sequnceType=TYPE_1_1_And_TYPE_1_2;
    }else{
        args->ODLength=1;
        args->M_sequnceType=TYPE_2_1;//见TableA.9 此时不在乎TYPE
    }
    
    args->M_squenceTime=CfgCycleTime[Port];//CfgCycleTime在通过SM_SetConfig之前已经被CM检验过
    
}
 
 //针对V1.1
void Create_DL_SetMode_Auguments_For_OPERATE_V1_1(DL_SetMode_ValueList_StructTypeDef * args){
     
    uint8_t  Operate_M_Squence_Code= (M_SquenceCapability&0x0E)>>1;
    //解析M-SquenceType
    switch (Operate_M_Squence_Code)
    {
    case 0:
        args->M_sequnceType=TYPE_2_1;
        args->ODLength=1;
         
        break;
    case 1:
        args->M_sequnceType=TYPE_1_1_And_TYPE_1_2;
        args->ODLength=1;
        breaks;
    case 4:
        args->M_sequnceType=TYPE_2_V;
        args->ODLength=1;
        break;
    case 5:
         args->M_sequnceType=TYPE_2_V;
         args->ODLength=2;
         break;
    case 6:
         args->M_sequnceType=TYPE_2_V;
         args->ODLength=8;
         break;
    case 7:
         args->M_sequnceType=TYPE_2_V;
         args->ODLength=32;
         break;
    
    default:
        break;
    }
    //解析PDInLen
    if(ProcessDataIn&0x80==0x80){ //说明ProcessDataIn长度是以字节为单位
        args->PDInputLength=ProcessDataIn&0x1F;
    }else{
        args->PDInputLength=(uint8_t)ceil((ProcessDataIn&0x1F)/8);
    }
    //解析PDOutLen
    if(ProcessDataOut&0x80==0x80){ //说明ProcessDataIn长度是以字节为单位
        args->PDOutputLength=ProcessDataOut&0x1F;
    }else{
        args->PDOutputLength=(uint8_t)ceil((ProcessDataOut&0x1F)/8);
    }
    
    args->M_squenceTime=CfgCycleTime[Port];//CfgCycleTime在通过SM_SetConfig之前已经被CM检验过
    
}



/*******************SM service**************************/
 

uint16_t  Service_SM_SetPortConfig_req(SM_SetPortConfig_Arg_TYPE ParameterList ){
     uint8_t Port=ParameterList.Port;
    if(SM_STATE[Port]==SM_InspectionFault_6||SM_STATE[Port]==SM_DIDO_8||SM_STATE==SM_PortInactive_0||SM_STATE[Port]==SM_wait_4||SM_STATE[Port]==SM_SMOperate_5){
        CfgCycleTime[Port]=ParameterList.ConfiguredCycleTime;
        TargetMode[Port]=ParameterList.TargetMode;
    
        IL[Port]=ParameterList.InspectionLevel;
        CVID[Port]=ParameterList.CVID;
        CDID[Port]=ParameterList.CDID;
        CRID[Port]=ParameterList.CRID;
        CFID[Port]=ParameterList.CFID;
        memcpy(&CSN[Port][0],&ParameterList.CSN[0],16);
        if(TargetMode[Port]==SM_INACTIVE){
            Event_SM_SetPortConfig_INACTIVE(Port);
            return 0;
        }
        if(TargetMode[Port]==SM_CFGCOM||TargetMode[Port]==SM_AUTOCOM){
           Event_SM_SetPortConfig_CFGCOM_AUTOCOM(Port);
           return 0;
        }
       
    }else{
        return 0x0001; //Parameter_Conflict
    }



}


uint16_t Service_SM_Operate_req(uint8_t Port){
    if(SM_STATE[Port]==SM_wait_4){
           SM_STATE[Port]=SM_SMOperate_5;
           SM_T12(Port);
       
    }
}


//向CM报告
void Service_SM_PortMode(uint8_t Port,SM_PortMode_Mode_TYPE Mode){
    if(Mode==SM_Mode_COMREADY){
        Event_SM_PortMode_COMREADY(Port);
    } 
    else if(Mode==SM_Mode_COMP_FAULT||Mode==SM_Mode_REVISION_FAULT||Mode==SM_Mode_SERNUM_FAULT||Mode==SM_Mode_CYCTIME_FAULT){
        Event_SM_PortMode_xFAULT(Port);
    }
    else if(Mode==SM_Mode_COMLOST){
        Event_SM_PortMode_COMLOST(Port);
    }
    else if(Mode==SM_Mode_SM_OPERATE){
        Event_SM_PortMode_OPERATE(Port);
    }else{
        return;
    }
}




